
// #include "glodef.h"
// #include "errlog.h"
#include "grid3d.h"
#include <stdlib.h>
#include <stdio.h>          /* file io */
#include <string.h>         /* memcpy */
#include <math.h>


/* Optimize for memory vs CPU time */
#define OPTIMIZE_MEM

#if defined(OPTIMIZE_CPU) && defined(OPTIMIZE_MEM)
/* both are defined -- not good */
#undef OPTIMIZE_CPU
#endif
#if (!defined(OPTIMIZE_CPU)) && (!defined(OPTIMIZE_MEM))
/* neither is defined -- not good */
#define OPTIMIZE_MEM
#endif

#ifdef OPTIMIZE_CPU
#ifndef STORE_MEASURES
#define STORE_MEASURES
#endif
#ifdef COMPUTE_MEASURES
#undef COMPUTE_MEASURES
#endif
#endif

#ifdef OPTIMIZE_MEM
#ifdef STORE_MEASURES
#undef STORE_MEASURES
#endif
#ifndef COMPUTE_MEASURES
#define COMPUTE_MEASURES
#endif
#endif

#ifdef NEED_MINJAC
#undef NEED_MINJAC
#endif

static inline double sqr(double A)
{
    return A * A;
}

static inline double mulvec(const double A[], const double B[])
{
    return A[0]*B[0]+A[1]*B[1]+A[2]*B[2];
}

static inline double crossp(const double *A, const double *B, double *P)
{
    P[0]=A[1]*B[2]-A[2]*B[1];
    P[1]=A[2]*B[0]-A[0]*B[2];
    P[2]=A[0]*B[1]-A[1]*B[0];
    return sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
}

double GridVolume(grid3d_t *grid)
{
    grid->Volume=0.0;
    for(int i=0; i<grid->NEL; i++)
    {
        elem_t *T=grid->elems+i;
        grid->Volume+=ComputeVolume(grid->nodes, T->nodes);
    }
    return grid->Volume;
}

/****************************************************************************
 *              ComputeXXX routines                                         *
 ****************************************************************************/

double ComputeJacobian(const node_t *nodes, const int *T_nodes)
{
    double x[4], y[4], z[4], vx[3], vy[3], vz[3];
    int i;
    for(i=0; i<4; i++)
    {
        x[i]=nodes[T_nodes[i]].coords[0];
        y[i]=nodes[T_nodes[i]].coords[1];
        z[i]=nodes[T_nodes[i]].coords[2];
    }
    for(i=1; i<4; i++)
    {
        vx[i-1]=x[i]-x[0];
        vy[i-1]=y[i]-y[0];
        vz[i-1]=z[i]-z[0];
    }
    return vx[0]*vy[1]*vz[2]+vx[1]*vy[2]*vz[0]+vx[2]*vy[0]*vz[1]
        -vx[0]*vy[2]*vz[1]-vx[1]*vy[0]*vz[2]-vx[2]*vy[1]*vz[0];
}

double ComputeVolume(const node_t *nodes, const int *T_nodes)
{
    return ComputeJacobian(nodes, T_nodes)/6.0;
}

void ComputeCentroid(const node_t *nodes, const int *T_nodes, double *pt)
{
    pt[0]=pt[1]=pt[2]=0.0;
    for(int j=0; j<3; j++)
    {
        for(int i=0; i<4; i++)
            pt[j]+=nodes[T_nodes[i]].coords[j];
        pt[j]*=.25;
    }
}

double ComputeArea(const node_t *nodes, const int *f_nodes, double *n)
{
    const double *p[3];
    double a[3], b[3], c[3], A;
    int i;
    for(i=0; i<3; i++)
        p[i]=nodes[f_nodes[i]].coords;
    for(i=0; i<3; i++)
    {
        a[i]=p[1][i]-p[0][i];
        b[i]=p[2][i]-p[0][i];
    }
    A=crossp(a, b, c);
    if(n)
    {
        for(i=0; i<3; i++)
            n[i]=c[i]/A;
    }
    return A*0.5;
}

double ComputeLength(const node_t *nodes, const int *e_nodes, double *t)
{
    const double *p[2];
    double c[3], l;
    int i;
    for(i=0; i<2; i++)
        p[i]=nodes[e_nodes[i]].coords;
    for(i=0; i<3; i++)
        c[i]=p[1][i]-p[0][i];
    l=sqrt(c[0]*c[0]+c[1]*c[1]+c[2]*c[2]);
    if(t)
        for(i=0; i<3; i++)
            t[i]=c[i]/l;
    return l;
}

double ElemQuality(const node_t *nodes, const int *T_nodes)
{
    double V, A[4], l[6], r, R, a, b, c, S, D;
    int n[4];
    /* volume of element */
    V=ComputeVolume(nodes, T_nodes);
    if(V<1e-16)
        return 0.0;
    /* lengths of edges */
    n[0]=T_nodes[0];
    n[1]=T_nodes[1];
    l[0]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[2];
    l[1]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[3];
    l[2]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[1];
    n[1]=T_nodes[2];
    l[3]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[1];
    n[1]=T_nodes[3];
    l[4]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[2];
    n[1]=T_nodes[3];
    l[5]=ComputeLength(nodes, n, NULL);
    /* areas of faces */
    n[0]=T_nodes[1];
    n[1]=T_nodes[2];
    n[2]=T_nodes[3];
    A[0]=ComputeArea(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[3];
    n[2]=T_nodes[2];
    A[1]=ComputeArea(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[1];
    n[2]=T_nodes[3];
    A[2]=ComputeArea(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[2];
    n[2]=T_nodes[1];
    A[3]=ComputeArea(nodes, n, NULL);
    /* compute r */
    S=A[0]+A[1]+A[2]+A[3];
    r=3.0*V/S;
    /* compute R */
    a=l[0]*l[5];
    b=l[1]*l[4];
    c=l[2]*l[3];
    S=0.5*(a+b+c);
    D=sqrt(S*(S-a)*(S-b)*(S-c));
    R=D/V*(1.0/3.0)*0.5;
    /* compute quality */
    return 3.0*r/R;
}

double NodeQuality(const grid3d_t *grid, const node_t *nodes, int nid)
{
    double MinQ, Q;
    int i;
    node_inv_t *ninv;

    ninv=grid->inverse->ninv+nid;
    for(MinQ=1.0, i=0; i<ninv->NEL; i++)
    {
        Q=ElemQuality(nodes, grid->elems[ninv->Tids[i]].nodes);
        MinQ=fmin(MinQ, Q);
    }
    return MinQ;
}

double GridQuality(const grid3d_t *grid, const node_t *nodes)
{
    double Q, MinQ;
    int i;
    elem_t *T;

    if(grid==NULL||nodes==NULL)
        return 0.0;

    for(MinQ=1.0, T=grid->elems, i=0; i<grid->NEL; i++, T++)
    {
        Q=ElemQuality(nodes, T->nodes);
        MinQ=fmin(MinQ, Q);
    }
    return MinQ;
}

double ElemSize(const node_t *nodes, const int *T_nodes)
{
    /* double D,S,c,a,b,R; */
    double V, l[6];
    int n[2];
    /* lengths of edges */
    n[0]=T_nodes[0];
    n[1]=T_nodes[1];
    l[0]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[2];
    l[1]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[0];
    n[1]=T_nodes[3];
    l[2]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[1];
    n[1]=T_nodes[2];
    l[3]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[1];
    n[1]=T_nodes[3];
    l[4]=ComputeLength(nodes, n, NULL);
    n[0]=T_nodes[2];
    n[1]=T_nodes[3];
    l[5]=ComputeLength(nodes, n, NULL);
    /* volume of element */
    V=ComputeVolume(nodes, T_nodes);
    if(V<1e-16)
        return 0.0;
    return fmax(fmax(fmax(fmax(fmax(l[0], l[1]), l[2]), l[3]), l[4]), l[5]);
    /* compute R -- circumradius*/
    /*
        a = l[0] * l[5];
        b = l[1] * l[4];
        c = l[2] * l[3];
        S = 0.5 * (a + b + c);
        D = sqrt(S*(S-a)*(S-b)*(S-c));
        R = D / V * THIRD * 0.5;
        return R;
     */
}

double GridSize(const grid3d_t *grid, const node_t *nodes)
{
    double H, MaxH;
    int i;
    if(grid==NULL||nodes==NULL)
        return 0.0;
    MaxH=0.0;
    for(i=0; i<grid->NEL; i++)
    {
        H=ElemSize(nodes, grid->elems[i].nodes);
        MaxH=fmax(MaxH, H);
    }
    return MaxH;
}

/****************************************************************************/

void ComputeGlobalCoord(const node_t *nodes, const int *Tnodes,
    const double *loc, double *glo)
{
    int i, j;
    double const *P[4];
    double B[4];

    /* the coordinates of element nodes go in P */
    for(i=0; i<4; i++)
        P[i]=nodes[Tnodes[i]].coords;

    /* the values of the basis functions at point loc go in B */
    B[0]=1.0-loc[0]-loc[1]-loc[2];
    B[1]=loc[0];
    B[2]=loc[1];
    B[3]=loc[2];

    /* compute the global coordinates in glo */
    glo[0]=glo[1]=glo[2]=0.0;
    for(i=0; i<3; i++)
        for(j=0; j<4; j++)
            glo[i]+=P[j][i]*B[j];

}

/****************************************************************************/

double elem_volume(const grid3d_t *grid, const elem_t *T)
#ifdef COMPUTE_MEASURES
{
    return ComputeVolume(grid->nodes, T->nodes);
}
#endif
#ifdef STORE_MEASURES
{
    (void)grid;
    return T->Volume;
}
#endif

double face_area(const grid3d_t *grid, const face_t *f, double *n)
#ifdef COMPUTE_MEASURES
{
    return ComputeArea(grid->nodes, f->nodes, n);
}
#endif
#ifdef STORE_MEASURES
{
    (void)grid;
    if(n)
        memcpy(n, f->n, 3*sizeof(double));
    return f->Area;
}
#endif

double face_vecs(const grid3d_t *grid, const face_t *f,
    double *t, double *s, double *n)
/* t & s are tangent and n is normal vector s.t. t,s,n forms an orthonormal
    basis with positive orientation. Returns the area of the face */
{
    double A, T[3], S[3], N[3];
    A=face_area(grid, f, N);
    edge_length(grid, grid->edges+f->edges[0], T);
    crossp(N, T, S);
    if(n) memcpy(n, N, sizeof(N));
    if(t) memcpy(t, T, sizeof(N));
    if(s) memcpy(s, S, sizeof(N));
    return A;
}

double edge_length(const grid3d_t *grid, const edge_t *e, double *t)
#ifdef COMPUTE_MEASURES
{
    return ComputeLength(grid->nodes, e->nodes, t);
}
#endif
#ifdef STORE_MEASURES
{
    (void)grid;
    if(t)
        memcpy(t, e->t, 3*sizeof(double));
    return e->Length;
}
#endif

/****************************************************************************/

void UpdateMeasures(grid3d_t *grid)
{
    (void)grid;
#if defined(NEED_MINJAC) || defined(STORE_MEASURES)
    if(grid->elems)
    {
        elem_t *T;
        grid->MINJAC=1e99;
        for(T=grid->elems; T<grid->elems+grid->NEL; T++)
        {
            double V=ComputeVolume(grid->nodes, T->nodes);
            grid->MINJAC=Fmin(grid->MINJAC, V);
#ifdef STORE_MEASURES
            T->Volume=V;
#endif
        }
    }
#endif
#ifdef STORE_MEASURES
    if(grid->faces)
    {
        face_t *f;
        for(f=grid->faces; f<grid->faces+grid->NF; f++)
            f->Area=ComputeArea(grid->nodes, f->nodes, f->n);
    }
    if(grid->edges)
    {
        edge_t *e;
        for(e=grid->edges; e<grid->edges+grid->NE; e++)
            e->Length=ComputeLength(grid->nodes, e->nodes, e->t);
    }
#endif
}

/****************************************************************************/


/****************************************************************************/
/*          elem_face_x functions                                           */
/****************************************************************************/

/*  These functions take as an argument an element (T) and a local index of
    face (fn) and return whatever _x is refering to */

int *elem_face_nodes(const int *Tnodes, int fn)
/* face nodes returned are ordered so that if passed to ComputeArea it will
    return normal outer to the element T */
{
    static int nds[3];
    switch(fn)
    {
        case 0:
            nds[0]=Tnodes[1];
            nds[1]=Tnodes[2];
            nds[2]=Tnodes[3];
            break;
        case 1:
            nds[0]=Tnodes[0];
            nds[1]=Tnodes[3];
            nds[2]=Tnodes[2];
            break;
        case 2:
            nds[0]=Tnodes[0];
            nds[1]=Tnodes[1];
            nds[2]=Tnodes[3];
            break;
        case 3:
            nds[0]=Tnodes[0];
            nds[1]=Tnodes[2];
            nds[2]=Tnodes[1];
            break;
    }
    return nds;
}

int *elem_face_nodes_local(const int *Tnodes, const int *fnodes)
{
    static int nds[3], i, j;
    for(i=0; i<3; i++)
    {
        for(j=0; j<4; j++)
        {
            if(Tnodes[j]==fnodes[i])
            {
                nds[i]=j;
                break;
            }
        }
        if(j>=4)
            return NULL;
    }
    return nds;
}

int elem_face_normal_sign(const grid3d_t *grid, const elem_t *T, int fn)
/* returns 1 if the normal to face fn of T is outer to T
    and -1 otherwise */
/* Remember: fn is local index of face */
{
    face_t *f;
    int *nds, j;
    nds=elem_face_nodes(T->nodes, fn);
    f=grid->faces+T->faces[fn];
    for(j=0; nds[j]!=f->nodes[0]; j++);
    j=(j+1)%3;
    if(nds[j]==f->nodes[1])
        return 1;
    else
        return -1;
}

double elem_face_area_normal(const grid3d_t *grid, const elem_t *T, int fn, double *VEC)
/* Remember: fn is local index of face */
#ifdef COMPUTE_MEASURES
{
    return ComputeArea(grid->nodes, elem_face_nodes(T->nodes, fn), VEC);
}
#endif
#ifdef STORE_MEASURES
{
    face_t *f;
    int sign;
    f=grid->faces+T->faces[fn];
    sign=elem_face_normal_sign(grid, T, fn);
    VEC[0]=sign*f->n[0];
    VEC[1]=sign*f->n[1];
    VEC[2]=sign*f->n[2];
    return f->Area;
}
#endif

/****************************************************************************/
/*                  edge_face_x functions                                   */

/****************************************************************************/

int edge_face_normal_sign(const grid3d_t *grid, const edge_t *e, const face_t *f)
/*  Returns 1 if the normal associated with the face f
    is in the direction needed for the div-free function associated with
    the edge.  Returns -1 if the face normal is in the opposite direction.
    Returns 0 if the face does not contain the edge.

    NOTE:  The direction of the normals for the div-free functions is
    chosen to comply with a right-hand rule.  If you point the thumb of
    your right hand in the direction of the edge vector (from e->nodes[0]
    to e->nodes[1], then the rest of your fingers are pointing in the direction
    of the normals, i.e. the normals are pointing counter-clockwise around the
    edge if edge is seen from above.
 */
{
    int i, en;
    en=e-grid->edges;
    for(i=0; i<3; i++)
        if(f->edges[i]==en)
            break;
    if(i>=3)
        return 0;

    if(f->nodes[0]==e->nodes[0]) /* edge starts at f->nodes[0] */
    {
        if(f->nodes[1]==e->nodes[1])
            return 1;
        else
            return -1;
    }
    else if(f->nodes[0]==e->nodes[1]) /* edge stops at f->nodes[0] */
    {
        if(f->nodes[2]==e->nodes[0])
            return 1;
        else
            return -1;
    }
    else /* f->nodes[0] is not on the edge */
    {
        if(f->nodes[1]==e->nodes[0])
            return 1;
        else
            return -1;
    }
}

double edge_face_coef_vect(const grid3d_t *grid, const edge_t *e,
    const face_t *f, double *V)
/*  returns the coefficient of the div-free function of edge e if expanded
    as a linear combination of the normal CR base functions.
    If V is not NULL it is loaded with the actual vector, i.e. after calling
        C = edge_face_coef_vect(grid, e, f, V);
    we have V = C * f->n.
 */
{
    int fn;
    int C;
    double T;
    C=edge_face_normal_sign(grid, e, f);
    if(C==0)
    {
        if(V)
            V[0]=V[1]=V[2]=0.0;
        return 0.0;
    }
    T=face_area(grid, f, V);
    T=C/T;
    if(V)
        for(fn=0; fn<3; fn++)
            V[fn]*=T;
    return T;
}



/****************************************************************************/
/*       local P1 matrices */

/****************************************************************************/

void local_mass_p1(const grid3d_t *grid, const elem_t *T, double ***M)
{
    int i, j;
    static double A[16], *AA[4]={A, A+4, A+8, A+12};
    double TV;
    if(M==NULL)
        return;
    else
        *M=AA;
    TV=elem_volume(grid, T);
    for(i=0; i<4; i++)
        for(j=0; j<4; j++)
            (*M)[i][j]=TV*(i==j?0.1:0.05);
}

void local_stif_p1(const grid3d_t *grid, const elem_t *T, double ***S)
{
    int i, j;
    static double B[16], *BB[4]={B, B+4, B+8, B+12};
    double A[4], n[4][3], TV;
    if(S==NULL)
        return;
    else
        *S=BB;
    /* compute area and normal to faces */
    for(i=0; i<4; i++)
        A[i]=elem_face_area_normal(grid, T, i, n[i]);
    /* now compute the local stiffness matrix */
    TV=elem_volume(grid, T);
    for(i=0; i<4; i++)
        for(j=0; j<4; j++)
            (*S)[i][j]=mulvec(n[i], n[j])*A[i]*A[j]/TV/9.0;
    return;
}

static void tensor_p1_p1_grad_p1(const grid3d_t *grid, const elem_t *T, double ****_A)
{
    static double A[3][4][4][4];
    static double *B[3*4*4]={
                             A[0][0][0], A[0][0][1], A[0][0][2], A[0][0][3],
                             A[0][1][0], A[0][1][1], A[0][1][2], A[0][1][3],
                             A[0][2][0], A[0][2][1], A[0][2][2], A[0][2][3],
                             A[0][3][0], A[0][3][1], A[0][3][2], A[0][3][3],
                             A[1][0][0], A[1][0][1], A[1][0][2], A[1][0][3],
                             A[1][1][0], A[1][1][1], A[1][1][2], A[1][1][3],
                             A[1][2][0], A[1][2][1], A[1][2][2], A[1][2][3],
                             A[1][3][0], A[1][3][1], A[1][3][2], A[1][3][3],
                             A[2][0][0], A[2][0][1], A[2][0][2], A[2][0][3],
                             A[2][1][0], A[2][1][1], A[2][1][2], A[2][1][3],
                             A[2][2][0], A[2][2][1], A[2][2][2], A[2][2][3],
                             A[2][3][0], A[2][3][1], A[2][3][2], A[2][3][3]
    };
    static double **C[3*4]={
                            B, B+4, B+8, B+12,
                            B+16, B+20, B+24, B+28,
                            B+32, B+36, B+40, B+44
    };
    double Q[4], n[4][3];
    int i, j, l, k;
    if(_A) *_A=C;
    else return;
    /* compute area and normal to faces */
    for(i=0; i<4; i++)
        Q[i]=ComputeArea(grid->nodes, elem_face_nodes(T->nodes, i), n[i]);
    /* (*_A)[i  ][j][k] contains phi_i**phi_j*d_x phi_k */
    /* (*_A)[i+4][j][k] contains phi_i**phi_j*d_y phi_k */
    /* (*_A)[i+8][j][k] contains phi_i**phi_j*d_z phi_k */
    for(i=0; i<4; i++) for(j=0; j<4; j++) for(k=0; k<4; k++)
            {
                for(l=0; l<3; l++)
                    A[l][i][j][k]=-(i==j?4:2)/(double)120*Q[k]*n[k][l];
            }
    return;
}

void local_advm_p1(const grid3d_t *grid, const elem_t *T, double **AV, double ***_M)
/*
 *  AV must be 3x4, i.e. A = sum_j (A[0][j], A[1][j], A[2][j])^T *phi_j
 */
{
    static double M[4][4];
    static double *MM[4]={M[0], M[1], M[2], M[3]};
    double ***A;
    int i, j, k, m;

    if(_M) *_M=MM;
    else return;

    tensor_p1_p1_grad_p1(grid, T, &A);
    memset(M, 0, sizeof(M));
    /* This loop fills up G_{ij}^a = int phi_i a.grad phi_j */
    for(i=0; i<4; i++) for(j=0; j<4; j++) for(k=0; k<4; k++) for(m=0; m<3; m++)
                    M[i][j]+=AV[m][k]*A[i+4*m][k][j];
    /* This loop fills up D_{ij}^a = int phi_i (div.a) phi_j */
    /*
        for(i=0;i<4;i++) for(j=0;j<4;j++) for(k=0;k<4;k++) for(m=0;m<3;m++)
                M[i][j] += 0.5*AV[m][k] * A[i+4*m][j][k];    
     */
    return;
}

void local_advm_D_p1(const grid3d_t *grid, const elem_t *T, double **AV, double ***_M)
/*
 *  AV must be 3x4, i.e. A = sum_j (A[0][j], A[1][j], A[2][j])^T *phi_j
 */
{
    static double M[4][4];
    static double *MM[4]={M[0], M[1], M[2], M[3]};
    double ***A;
    int i, j, k, m;

    if(_M) *_M=MM;
    else return;

    tensor_p1_p1_grad_p1(grid, T, &A);
    memset(M, 0, sizeof(M));
    /* This loop fills up D_{ij}^a = 0.5*int phi_i (div.a) phi_j */
    for(i=0; i<4; i++) for(j=0; j<4; j++) for(k=0; k<4; k++) for(m=0; m<3; m++)
                    M[i][j]+=0.5*AV[m][k]*A[i+4*m][j][k];

    return;
}


/****************************************************************************/
/*       local CR matrices */

/****************************************************************************/

void local_mass_cr(const grid3d_t *grid, const elem_t *T, double ***M)
{
    int i, j;
    static double A[16], *AA[4]={A, A+4, A+8, A+12};
    double TV;
    if(M==NULL)
        return;
    else
        *M=AA;
    TV=elem_volume(grid, T);
    for(i=0; i<4; i++)
        for(j=0; j<4; j++)
            (*M)[i][j]=TV*(i==j?0.4:-0.05);
}

void local_stif_cr(const grid3d_t *grid, const elem_t *T, double ***S)
{
    /* NOTE: the local cr stiffness matrix is equal to 9 times the
        local p1 stiffness matrix.  see basis3d_locmat.mws */
    int i, j;
    static double B[16], *BB[4]={B, B+4, B+8, B+12};
    double A[4], n[4][3], TV;
    /* double *p[4], a[3], b[3]; */
    /* node_t *nodes = grid->nodes; */
    if(S==NULL)
        return;
    else
        *S=BB;
    /* area and normal to faces */
    for(i=0; i<4; i++)
        A[i]=elem_face_area_normal(grid, T, i, n[i]);
    TV=elem_volume(grid, T);
    /* now compute the local stiffness matrix */
    for(i=0; i<4; i++)
        for(j=0; j<4; j++)
            (*S)[i][j]=mulvec(n[i], n[j])*A[i]*A[j]/TV;
    return;
}

void local_div_cr(const grid3d_t *grid, const elem_t *T, double ***D)
{
    int i;
    static double A[4], *AA[1]={A};
    double F;
    int S;
    if(D==NULL)
        return;
    else
        *D=AA;
    for(i=0; i<4; i++)
    {
        F=face_area(grid, grid->faces+T->faces[i], NULL);
        S=elem_face_normal_sign(grid, T, i);
        (*D)[0][i]=S * F;
    }
}

void local_div_cr_xyz(const grid3d_t *grid, const elem_t *T, double ***D)
{
    int i;
    static double A[12], *AA[1]={A};
    double F, N[3];
    int S;
    if(D==NULL)
        return;
    else
        *D=AA;
    for(i=0; i<4; i++)
    {
        F=face_area(grid, grid->faces+T->faces[i], N);
        S=elem_face_normal_sign(grid, T, i);
        (*D)[0][i]=S*F*N[0];
        (*D)[0][i+4]=S*F*N[1];
        (*D)[0][i+8]=S*F*N[2];
    }
}

/* the following routine is mesmerizing */
void local_mass_df(const grid3d_t *grid, const elem_t *T,
    double ***M, double ***N)
{
    int i, j, k, l;
    static double B[36], *BB[6]={B, B+6, B+12, B+18, B+24, B+30};
    static double C[24], *CC[6]={C, C+4, C+8, C+12, C+16, C+20};

    double f_n[4][3], f_A[4], NN[4][4], crm[4][4];
    double T_V;
    int efS[6][4];

    edge_t *e;
    if((M==NULL)&&(N==NULL)) return;
    if(M) *M=BB;
    if(N) *N=CC;

    memset(B, 0, sizeof(B));
    memset(C, 0, sizeof(C));

    for(i=0; i<4; i++)
        f_A[i]=face_area(grid, grid->faces+T->faces[i], f_n[i]);

    for(i=0; i<4; i++)
    {
        NN[i][i]=1.0/sqr(f_A[i]);
        for(j=i+1; j<4; j++)
            NN[i][j]=NN[j][i]=mulvec(f_n[i], f_n[j])/f_A[i]/f_A[j];
    }

    T_V=elem_volume(grid, T);
    for(i=0; i<4; i++)
    {
        crm[i][i]=0.4*T_V;
        for(j=i; j<4; j++)
            crm[i][j]=crm[j][i]= -0.05*T_V;
    }
#define CRM(i,j)        crm[i][j]
    /* #define CRM(i,j)    (T_V*((i)==(j)?0.4:-0.05)) */

    for(i=0; i<6; i++)
    {
        e=grid->edges+T->edges[i];
        for(j=0; j<4; j++)
        {
            efS[i][j]=edge_face_normal_sign(grid, e, grid->faces+T->faces[j]);
        }
    }

    if(M)
    {
        for(i=0; i<6; i++)
            for(j=i; j<6; j++)
            {
                for(k=0; k<4; k++)
                    for(l=0; l<4; l++)
                        (*M)[i][j]+=(efS[i][k]*efS[j][l])*NN[k][l]*CRM(k, l);
                (*M)[j][i]=(*M)[i][j];
            }
    }

    if(N)
    {
        for(i=0; i<6; i++)
            for(j=0; j<4; j++)
                for(k=0; k<4; k++)
                    (*N)[i][j]+=efS[i][k]*NN[k][j]*f_A[j]*CRM(k, j);

    }
#undef CRM
}

/* =========================================================================
            get_local_tsn and local_mass_cr_tsn
   =========================================================================
    local_mass_cr_tsn computes the local cr mass matrix in t-s-n basis
    as the matrix product
                C  *  LM  *  C^T,
    where LM is the local cr mass matrix in x-y-z, and C is t-s-n 2 x-y-z
    conversion matrix.  All three matrices are 12x12.  LM is block
    diagonal, with 3 4x4 1d-mass matrix blocks on the diagonal.
    C is 3x3 block  matrix, each block being diagonal 4x4 matrix:
                (   Tx  Ty  Tz  )
            C = (   Sx  Sy  Sz  ).
                (   Nx  Ny  Nz  )
    Here Pq is a 4x4 diagonal matrix containing the q-coordinates of the
    P-vectors on the diagonal, e.g.
                    (   T1x 0   0   0   )
                    (   0   T2x 0   0   )
            Tx  =   (   0   0   T3x 0   )
                    (   0   0   0   T4x )
    The C matrix is produced by get_local_tsn.
   ========================================================================= */
static void get_local_tsn(const grid3d_t *grid, const elem_t *T, double ***TSN)
{
    static double _tsn[12][12], *C[12], t[3], s[3], n[3];
    int i;

    if(TSN) *TSN=C;
    for(i=0; i<12; i++) C[i]=_tsn[i];
    memset(_tsn, 0, sizeof(_tsn));
    for(i=0; i<4; i++)
    {
        face_vecs(grid, grid->faces+T->faces[i], t, s, n);
        C[i][i]=t[0];
        C[i][i+4]=t[1];
        C[i][i+8]=t[2];
        C[i+4][i]=s[0];
        C[i+4][i+4]=s[1];
        C[i+4][i+8]=s[2];
        C[i+8][i]=n[0];
        C[i+8][i+4]=n[1];
        C[i+8][i+8]=n[2];
    }
}

void local_mass_cr_tsn(const grid3d_t *grid, const elem_t *T, double ***M)
{
    static double _matr[12][12], *_M[12], P[12][12];
    double **lM, **C;
    int i, j, k;

    for(i=0; i<12; i++) _M[i]=_matr[i];
    if(M) *M=_M;
    memset(_matr, 0, sizeof(_matr));
    memset(P, 0, sizeof(P));
    local_mass_cr(grid, T, &lM);
    get_local_tsn(grid, T, &C);
    for(i=0; i<4; i++)
        for(j=0; j<4; j++)
            _M[i][j]=_M[i+4][j+4]=_M[i+8][j+8]=lM[i][j];
    for(i=0; i<12; i++)
        for(j=0; j<12; j++)
            for(k=0; k<12; k++)
                P[i][j]+=C[i][k]*_M[k][j];
    memset(_matr, 0, sizeof(_matr));
    for(i=0; i<12; i++)
        for(j=0; j<12; j++)
            for(k=0; k<12; k++)
                _M[i][j]+=P[i][k]*C[j][k];
    return;
}

/* =========================================================================
            get_local_efc and local_mass_tsdf
   ========================================================================= */

static void get_local_efc(const grid3d_t *grid, const elem_t *T, double ***EFC)
{
    static double _efc[14][12], *E[14];
    edge_t*e[6];
    face_t*f[4];
    int i, j;

    if(EFC) *EFC=E;
    for(i=0; i<14; i++) E[i]=_efc[i];
    memset(_efc, 0, sizeof(_efc));
    for(i=0; i<6; i++) e[i]=grid->edges+T->edges[i];
    for(j=0; j<4; j++) f[j]=grid->faces+T->faces[j];
    for(i=0; i<8; i++) E[i][i]=1.0;
    for(i=0; i<6; i++)
        for(j=0; j<4; j++)
            E[i+8][j+8]=edge_face_coef_vect(grid, e[i], f[j], NULL);
}

void local_mass_tsdf(const grid3d_t *grid, const elem_t *T, double ***M)
{
    static double _matr[14][14], *_M[14], P[14][12];
    double **LM, **E;
    int i, j, k;

    for(i=0; i<14; i++) _M[i]=_matr[i];
    if(M) *M=_M;
    local_mass_cr_tsn(grid, T, &LM);
    get_local_efc(grid, T, &E);
    /* multiply E * LM = P */
    memset(P, 0, sizeof(P));
    for(i=0; i<14; i++)
        for(j=0; j<12; j++)
            for(k=0; k<12; k++)
                P[i][j]+=E[i][k]*LM[k][j];
    /* multiply P * E^T = M */
    memset(_matr, 0, sizeof(_matr));
    for(i=0; i<14; i++)
        for(j=0; j<14; j++)
            for(k=0; k<12; k++)
                _matr[i][j]+=P[i][k]*E[j][k];
    return;
}


